#include "task.h"
#include "list.h"

static const int8_t prio_table[256]=
{
	-1,0, 1, 1, 2, 2, 2, 2, 3, 3, 3, 3, 3, 3, 3, 3,
	4, 4, 4, 4, 4, 4, 4, 4, 4, 4, 4, 4, 4, 4, 4, 4,
	5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5,
	5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5, 5,
	6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6,
	6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6,
	6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6,
	6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6, 6,
	7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7,
	7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7,
	7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7,
	7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7,
	7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7,
	7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7,
	7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7,
	7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7, 7,
};

typedef struct {
	tcb_list_t  run[8]; 		// Running state
	tcb_list_t  wait[1]; 		// Waiting state
	tcb_list_t  pend[1]; 		// Pending state
	task_t  	*curr_task; 	// Current running task
	uint8_t     prio_bitmap; 	// Priority bitmap
}tasks_t;

static tasks_t  __tasks={0};

void debug_task_list(void)
{
	// debug("__tasks:%p, wait:%p, pend:%p, curr_task:%p, prio_bitmap:%0X\n",&__tasks , __tasks.wait, __tasks.pend, __tasks.curr_task, __tasks.prio_bitmap);
	for (int i = 0; i < 8; i++) {
		for (tcb_node_t *task = __tasks.run[i].head; task != NULL; task = task->next) {
			debug("run[%d]:%p, timeout:%d\n", i, task, task->tcb->timeout);
		}
	}
	for (tcb_node_t *task = __tasks.wait[0].head; task != NULL; task = task->next) {
		debug("wait[0]:%p, timeout:%d\n", task, task->tcb->timeout);
	}
	for (tcb_node_t *task = __tasks.pend[0].head; task != NULL; task = task->next) {
		debug("pend[0]:%p, timeout:%d\n", task, task->tcb->timeout);
	}
	debug("__tasks.prio_bitmap:%0X\n", __tasks.prio_bitmap);
}

// idle task
__weak void task_idleTask(void)
{
	uint32_t sys_sleep_timer = (uint32_t)(-1);
}

// task scheduler process
void task_sched_process(void)
{
	uint32_t curr_time = BeeGetTick();
	tcb_node_t *task_node_prev = NULL;
	for (tcb_node_t *task_node = __tasks.wait[0].head; task_node != NULL;) {
		if (((uint32_t)(curr_time - task_node->tcb->timeout) <= ((uint32_t)(-1) >> 1))) {
			task_node_prev = task_node->prev;
			list_remove(task_node->tcb->state_list, &task_node->tcb->node); // remove from wait list
			task_node->tcb->state_list = &__tasks.run[task_node->tcb->prio];
			list_insert_tail(task_node->tcb->state_list, &task_node->tcb->node); // insert to run list
			__tasks.prio_bitmap |= (1 << task_node->tcb->prio);
			if(task_node_prev == NULL){
				task_node = __tasks.wait[0].head;
			}else{
				task_node = task_node_prev->next;
			}
		}else{
			task_node = task_node->next;
		}
	}
	if (__tasks.prio_bitmap == 0) { // idle task
		task_idleTask();
	} else { // Run all tasks of the highest priority
		uint8_t prio_bitmap_last = __tasks.prio_bitmap;
		for (tcb_node_t *task_node_prev = NULL, *task_node = __tasks.run[prio_table[__tasks.prio_bitmap]].head; task_node != NULL; ) { // Run all tasks of this priority
			checkACCESS(task_node->tcb->task_func);
			__tasks.curr_task = task_node->tcb;
			task_node->tcb->task_func(task_node->tcb->arg);
			if ((task_node->tcb->prio < prio_table[__tasks.prio_bitmap]) || ((__tasks.prio_bitmap & (1 << prio_table[prio_bitmap_last])) == 0)) {
				task_node = NULL;
			} else {
				if (task_node_prev == NULL){
					if (task_node != __tasks.run[prio_table[__tasks.prio_bitmap]].head) {
						task_node = __tasks.run[prio_table[__tasks.prio_bitmap]].head;
					} else {
						task_node_prev = task_node;
						task_node = task_node->next;
					}
				} else {
					task_node_prev = task_node_prev->next;
					task_node = task_node_prev->next;
				}
			}
		}
	}
}

task_t* task_create(task_t* new_task, void (*task_func)(void *), void *arg, task_prio_e prio)
{
	if (task_func == NULL) {
		return NULL;
	} else {
		if (new_task == NULL) {
			new_task = (task_t*)(BeeMalloc(sizeof(task_t)));
			if (new_task == NULL) {
				return NULL;
			}
			new_task->malloc_type = 1; // malloc
		}
		new_task->arg = arg;
		new_task->task_func = task_func;
		new_task->prio = (prio >= 7) ? 7 : prio;
		new_task->notify_state = NOTIFY_NONE;
		new_task->notify_value = 0;
		new_task->timeout = 0;
		new_task->state_list = &__tasks.run[prio];
		new_task->node = (tcb_node_t){
			.prev = NULL,
			.next = NULL,
			.tcb = new_task
		};
		list_insert_tail(new_task->state_list, &new_task->node);
		__tasks.prio_bitmap |= (1 << prio);
	}
	return new_task;
}

int task_delete(task_t* task)
{
	if (task == NULL) {
		task = __tasks.curr_task;
		if (task == NULL) {
			return retERROR;
		}
	}
	list_remove(task->state_list, &task->node);
	if ((task->state_list == &__tasks.run[task->prio]) && (task->state_list->head == NULL)) {
		__tasks.prio_bitmap &= ~(1 << task->prio);
	}
	if (task->malloc_type == 1) { // malloc
		BeeFree(task);
	}	
	if (task == __tasks.curr_task) {
		__tasks.curr_task = NULL;
	}

	return retTRUE;
}

int task_sleep(task_t* task, uint32_t timeout)
{
	uint32_t curr_time = BeeGetTick();

	if (task == NULL) {
		task = __tasks.curr_task;
		if (task == NULL) {
			return retERROR;
		}
	}
	if (task->state_list == &__tasks.run[task->prio]) {
		list_remove(task->state_list, &task->node); // remove from run list
		if (task->state_list->head == NULL) {
			__tasks.prio_bitmap &= ~(1 << task->prio);
		}
		task->state_list = &__tasks.wait[0];
		task->timeout = curr_time + timeout; // set timeout
		list_insert_tail(task->state_list, &task->node); // insert to wait list
	}else if (task->state_list == &__tasks.wait[0]){
		task->timeout = curr_time + timeout;
	}
	return retTRUE;
}

int task_suspend(task_t* task)
{
	if (task == NULL) {
		task = __tasks.curr_task;
		if (task == NULL) {
			return retERROR;
		}
	}
	if (task->state_list != &__tasks.pend[0]) {
		list_remove(task->state_list, &task->node); // remove from wait/run list
		if ((task->state_list == &__tasks.run[task->prio]) && (task->state_list->head == NULL)) {
			__tasks.prio_bitmap &= ~(1 << task->prio);
		}
		task->timeout = 0;
		task->state_list = &__tasks.pend[0];
		list_insert_tail(task->state_list, &task->node); // insert to pend list
	}
	return retTRUE;
}

int task_wakeup(task_t* task)
{
	if ((task == NULL)||(task->state_list == &__tasks.run[task->prio])) {
		return retFALSE;
	} else {
		list_remove(task->state_list, &task->node); // remove from wait list
		task->timeout = BeeGetTick();
		task->state_list = &__tasks.run[task->prio];
		list_insert_tail(task->state_list, &task->node); // insert to run list
		__tasks.prio_bitmap |= (1 << task->prio);
	}
	return retTRUE;
}

int task_notifyWait(uint32_t Entry_CleanBits, uint32_t Exit_CleanBits, uint32_t *notify_value, uint32_t timeout)
{
	int ret = retTRUE;
	task_t *task = __tasks.curr_task;
	if (task == NULL) {
		return retERROR;
	}
		
	if(task->notify_state != NOTIFY_RECE){
		if((task->notify_state == NOTIFY_WAIT) && (task->timeout != 0) && (((uint32_t)(BeeGetTick() - task->timeout) <= ((uint32_t)(-1) >> 1)))){
			ret = retTIMEOUT; // timeout
		}else{
			ret = retBLOCKED; 
		}
		task->notify_state = NOTIFY_WAIT;
		task->notify_value &= ~Entry_CleanBits; // clean bits
	}else{
		if (notify_value){
			*notify_value = task->notify_value;
		}
		task->notify_value &= ~Exit_CleanBits; // clean bits
		task->notify_state = NOTIFY_NONE;
	}
	if(timeout == configMAX_DELAY) {
		task_suspend(task);
	}else{
		task_sleep(task, timeout);
	}
	return ret;
}

int task_notify(task_t* task, uint32_t value, notify_action_e action, uint32_t *old_value)
{
	checkACCESS(task);

	int ret = retTRUE;
	if (old_value){
		*old_value = task->notify_value;
	}
	task->notify_state = NOTIFY_RECE;
	switch (action){
		case ACTION_NO:
			; // do nothing
			break;
		case ACTION_SET_BITS:
			task->notify_value |= value;
			break;
		case ACTION_INCREMENT:
			task->notify_value++;
		break;
		case ACTION_OVERWRITE:
			task->notify_value = value;
			break;
		case ACTION_OVERWRITE_CHECK:
			if(task->notify_state != NOTIFY_RECE){
				task->notify_value = value;
			}else{
				ret = retFALSE;
			}
			break;
		default:
			ret = retFALSE;
			break;
	}
	task_wakeup(task);
	return ret;
}

int task_set_prio(task_t* task, task_prio_e prio)
{
	if ((task == NULL)||(task->prio == prio)) {
		return -1;
	} else {
		if (task->state_list == &__tasks.run[task->prio]) {
			list_remove(task->state_list, &task->node); // remove from run list
			if (task->state_list->head == NULL) {
				__tasks.prio_bitmap &= ~(1 << task->prio);
			}
			task->prio = (prio >= 7) ? 7 : prio;
			task->state_list = &__tasks.run[prio];
			list_insert_tail(task->state_list, &task->node); // insert to run list
			__tasks.prio_bitmap |= (1 << prio);
		} else {
			task->prio = (prio >= 7) ? 7 : prio;
		}
	}
	return 0;
}